/* MCPWM basic config example

   This example code is in the Public Domain (or CC0 licensed, at your option.)

   Unless required by applicable law or agreed to in writing, this
   software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
   CONDITIONS OF ANY KIND, either express or implied.
*/

/*
 * This example will show you how to use each submodule of MCPWM unit.
 * The example can't be used without modifying the code first.
 * Edit the macros at the top of mcpwm_example_basic_config.c to enable/disable the submodules which are used in the example.
 */

#include <stdio.h>
#include "string.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "esp_attr.h"
#include "soc/rtc.h"
#include "driver/mcpwm.h"
#include "soc/mcpwm_periph.h"

#include "esp_log.h"

#define CONFIG_STEP_MOTOR_CTRL_FREQ   250

#define LEFT_MOTOR_PWM_PIN 2
#define LEFT_MOTOR_DIR_PIN 5

#define RIGHT_MOTOR_PWM_PIN 15
#define RIGHT_MOTOR_DIR_PIN 19

#define BOTTOM_MOTOR_PWM_PIN 4
#define BOTTOM_MOTOR_DIR_PIN 18

typedef struct {
  uint32_t dir_pin;
  uint32_t pwm_pin;
  mcpwm_unit_t pwm_unit;
  mcpwm_timer_t pwm_timer;
  mcpwm_generator_t pwm_chan;
} motor_info_t;

static const motor_info_t motors[3] = {
  {LEFT_MOTOR_DIR_PIN, LEFT_MOTOR_PWM_PIN, MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A},
  {RIGHT_MOTOR_DIR_PIN, RIGHT_MOTOR_PWM_PIN, MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B},
  {BOTTOM_MOTOR_DIR_PIN, BOTTOM_MOTOR_PWM_PIN, MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A},
};

#define TIM0_TEP_INT_EN BIT(6)  //PWM timer 0 interrupt bit
#define TIM1_TEP_INT_EN BIT(7)  //PWM timer 1 interrupt bit
#define TIM2_TEP_INT_EN BIT(8)  //PWM timer 2 interrupt bit
static mcpwm_dev_t *MCPWM[2] = {&MCPWM0, &MCPWM1};

typedef struct {
  uint32_t enable;
  uint32_t steps;
} motor_ctrl_t;

static motor_ctrl_t motor_ctrls[3] = {
  {0, 0},
  {0, 0},
  {0, 0},
};

typedef struct {
  uint32_t idx;
  uint32_t evt;
} motor_evt_t;

xQueueHandle evt_queue;

static void handle_steps(int idx)
{
  if(idx >= 3) return;
  motor_evt_t evt;
  if(motor_ctrls[idx].enable) {
    if(motor_ctrls[idx].steps > 0) {
      motor_ctrls[idx].steps --;
      if(motor_ctrls[idx].steps == 0) {
        // notify tasks ...
        evt.idx = idx;
        xQueueSendFromISR(evt_queue, &evt, NULL);
      }
    }
  }
}

/**
 * @brief this is ISR handler function, here we check for interrupt that triggers rising edge on CAP0 signal and according take action
 */
static void IRAM_ATTR isr_handler(void *arg)
{
  uint32_t mcpwm_intr_status = MCPWM[MCPWM_UNIT_0]->int_st.val; //Read interrupt status
  if(mcpwm_intr_status & TIM0_TEP_INT_EN) {
    handle_steps(0);
    handle_steps(1);
  }
  if(mcpwm_intr_status & TIM1_TEP_INT_EN) {
    handle_steps(2);
  }
  MCPWM[MCPWM_UNIT_0]->int_clr.val = mcpwm_intr_status;
}

static void arm_ctrl_init(void)
{
  //1. MCPWM GPIO initialization
  printf("initializing mcpwm gpio...\n");
  mcpwm_pin_config_t pin_config = {
    .mcpwm0a_out_num = LEFT_MOTOR_PWM_PIN,
    .mcpwm0b_out_num = RIGHT_MOTOR_PWM_PIN,
    .mcpwm1a_out_num = BOTTOM_MOTOR_PWM_PIN,
  };
  mcpwm_set_pin(MCPWM_UNIT_0, &pin_config);

  gpio_pad_select_gpio(LEFT_MOTOR_DIR_PIN);
  gpio_pad_select_gpio(RIGHT_MOTOR_DIR_PIN);
  gpio_pad_select_gpio(BOTTOM_MOTOR_DIR_PIN);
  gpio_set_direction(LEFT_MOTOR_DIR_PIN, GPIO_MODE_OUTPUT);
  gpio_set_direction(RIGHT_MOTOR_DIR_PIN, GPIO_MODE_OUTPUT);
  gpio_set_direction(BOTTOM_MOTOR_DIR_PIN, GPIO_MODE_OUTPUT);
  gpio_set_level(LEFT_MOTOR_DIR_PIN, 0);
  gpio_set_level(RIGHT_MOTOR_DIR_PIN, 0);
  gpio_set_level(BOTTOM_MOTOR_DIR_PIN, 0);

  //2. initialize MCPWM configuration
  printf("Configuring Initial Parameters of mcpwm...\n");
  mcpwm_config_t pwm_config;
  pwm_config.frequency = CONFIG_STEP_MOTOR_CTRL_FREQ; //frequency = 250Hz
  pwm_config.cmpr_a = 0.0;                            //duty cycle of PWMxA = 50.0%
  pwm_config.cmpr_b = 0.0;                            //duty cycle of PWMxb = 50.0%
  pwm_config.counter_mode = MCPWM_UP_COUNTER;         // up counter
  pwm_config.duty_mode = MCPWM_DUTY_MODE_0;
  mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings
  mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_1, &pwm_config); //Configure PWM1A & PWM1B with above settings
  MCPWM[MCPWM_UNIT_0]->int_ena.val = TIM0_TEP_INT_EN | TIM1_TEP_INT_EN; //Enable interrupt on TEP0 TEP1
  mcpwm_isr_register(MCPWM_UNIT_0, isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL);  //Set ISR Handler
}

static void arm_event_handle(void *arg)
{
  motor_evt_t evt;
  uint32_t idx;
  printf("arm event handler\n");
  while(1) {
    xQueueReceive(evt_queue, &evt, portMAX_DELAY);
    idx = evt.idx;
    motor_ctrls[idx].enable = 0;
    mcpwm_set_duty(motors[idx].pwm_unit, motors[idx].pwm_timer, motors[idx].pwm_chan, 0);
  }
}

static uint32_t motor_running(uint32_t idx) { return motor_ctrls[idx].enable; }

static void motor_run_steps(uint32_t idx, uint32_t step, uint32_t dir)
{
  if(idx < 3 && step > 0) {
    gpio_set_level(motors[idx].dir_pin, dir);
    motor_ctrls[idx].enable = 1;
    motor_ctrls[idx].steps = step;
    mcpwm_set_duty(motors[idx].pwm_unit, motors[idx].pwm_timer, motors[idx].pwm_chan, 50);
  } else {
    mcpwm_set_duty(motors[idx].pwm_unit, motors[idx].pwm_timer, motors[idx].pwm_chan, 0);
    motor_ctrls[idx].enable = 0;
    motor_ctrls[idx].steps = 0;
  }
}

#define START_BETA_0 1.57079632
#define ARM_RADIUS_0 10.0f

static float tar_x = 0, tar_y = 0;
static int left_pos = 0, right_pos = 0;

#include <math.h>
static void arm_ctrl_task(void *arg)
{
  uint8_t left_ctrl_dir = 1, right_ctrl_dir = 0;
  uint32_t left_run_step = 0, right_run_step = 0;
  int left_tar_pos = 0, right_tar_pos = 0;

  float d0 = ARM_RADIUS_0 * cosf(START_BETA_0 / 2) * 2;
  float rou, item_0, item_1;

  arm_ctrl_init();
  while(1) {
    vTaskDelay(1);

    rou = sqrtf(tar_x * tar_x + (tar_y + d0) * (tar_y + d0));
    item_0 = acosf(rou / (ARM_RADIUS_0 * 2));
    item_1 = asinf(tar_x / rou);
    left_tar_pos = (item_1 - item_0 + START_BETA_0 / 2) * 200;
    right_tar_pos = (item_1 + item_0 - START_BETA_0 / 2) * 200;

    left_run_step = abs(left_tar_pos - left_pos);
    left_ctrl_dir = left_tar_pos > left_pos ? 1 : 0;
    if(!motor_running(0) && left_run_step > 0) {
      motor_run_steps(0, left_run_step, left_ctrl_dir ? 1 : 0);
      if(left_ctrl_dir) left_pos += left_run_step;
      else left_pos -= left_run_step;
    }
    right_run_step = abs(right_tar_pos - right_pos);
    right_ctrl_dir = right_tar_pos > right_pos ? 1 : 0;
    if(!motor_running(1) && right_run_step > 0) {
      motor_run_steps(1, right_run_step, right_ctrl_dir ? 1 : 0);
      if(right_ctrl_dir) right_pos += right_run_step;
      else right_pos -= right_run_step;
    }
  }
}

static void arm_pos_task(void *arg)
{
  float theta = 0;
  printf("start pos\n");
  while(1) {
    vTaskDelay(1);
    theta ++;
    if(theta >= 360) theta = 0;
    tar_x = 1 * cosf(theta * 0.017453293f);
    tar_y = 1 * sinf(theta * 0.017453293f) + 1;
  }
}

void app_main(void)
{
    printf("ARM control started ...\n");
    evt_queue = xQueueCreate(10, sizeof(motor_evt_t));
    xTaskCreate(arm_event_handle, "arm_handle", 4096, NULL, 5, NULL);
    xTaskCreate(arm_ctrl_task, "arm_ctrl", 4096, NULL, 5, NULL);
    vTaskDelay(100);
    xTaskCreate(arm_pos_task, "arm_pos", 4096, NULL, 5, NULL);
}
